#include <cmath>
#include <iostream>
#include <thread>

#include "asio_driver/device/buzzer_src.hpp"
#include "asio_driver/device/rc_sbus.hpp"
#include "asio_driver/io/timer.hpp"
#include "asio_driver/utils/range.hpp"
#include "asio_driver_special/motor_brt_can.hpp"
#include "ctrl_alg/pid/pid_pos.hpp"

static constexpr int rate = 200;

unsigned char connectCommand[2] = "c";
unsigned char disconnectCommand[2] = "d";

int main()
{
    std::cout << "init device!" << std::endl;
    AD::AsioContext::SharePtr context = AD::AsioContext::CreatAsioContext();
    std::unique_ptr<AD::Can> can = std::make_unique<AD::Can>(context);
    AD::CanBus::SharePtr can_bus = AD::CanBus::Create(context);
    AD::Special::MotorBrtCan motorLeft = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorRight = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorLeftForward = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorRightForward = AD::Special::MotorBrtCan(context);

    AD::RC_Sbus sbus = AD::RC_Sbus(context);
    AD::Buzzer buzzer = AD::Buzzer(context);

    motorLeft.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 4, .poleCount = 14});
    motorRight.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 3, .poleCount = 14});
    motorLeftForward.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 1, .poleCount = 14});
    motorRightForward.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 2, .poleCount = 14});

    can->Open("can0");
    can_bus->Open(std::move(can));
    sbus.Open("/dev/ttyS7");
    buzzer.Open(34, true);
    
    buzzer.Beep();
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    buzzer.StopBeep();
    std::cout << "start control!" << std::endl;

    bool checkRc = true;

    while (context->Running())
    {
        context->SpinOnce();
        // continue;
        if (checkRc)
        {
            auto rcFrame = sbus.ReadFrame();
            if (rcFrame.IsDisconnect())
            {
                std::cerr << "sbus disconnect" << std::endl;
                continue;
            }
            checkRc = false;
            std::cout << "sbus mode reset, check finished" << std::endl;
        }
        // Get input
        auto rcFrame = sbus.ReadFrame();
        float forwardControl = rcFrame.Channel(1, -15000, 15000);
        float rotateControl = rcFrame.Channel(0, -3000, 3000);

        float left = forwardControl+rotateControl;
        float right = forwardControl-rotateControl;
        motorLeft.SendHeartBeat();
        motorRight.SendHeartBeat();
        motorLeft.SetCurrent(left);
        motorRight.SetCurrent(right);

        context->WaitRate(rate);
    }
    return 0;
}